%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Sajad Saeedi
% University of New Brunswick
% Copy Right 2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ref: 
% S. Saeedi, L. Paull, M. Trentini, M. Seto, and H. Li, 
% Map Merging Using Hough Peak Matching, 
% Proceedings of the IEEE/RSJ International Conference on 
% Intelligent Robots and Systems (IROS), 
% Vilamoura, Portugal, October 7-12, pp. 4683 - 4688.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 返回值
% D：rho，沿垂直于线条的向量从原点到线条的距离，以数值数组形式返回。
% T：theta，x 轴和 rho 向量之间的角度，以度为单位，以数值矩阵形式返回。
% R：-H，它是一个参数空间矩阵，其行和列分别对应于 rho 和 theta 值。
% ang：最小的角度值在矩阵中的索引
function [ang, R, T, D] = align_roden(map)
Debug = 0;
I = map;
%figure;
%imshow(I)
%[R,xp] = radon(I,[0 45]);
%figure; plot(xp,R(:,1)); title('R_{0^o} (x\prime)')
%figure; plot(xp,R(:,2)); title('R_{45^o} (x\prime)')

t2=95;
t1=85;
st=.1;
% theta = t1:st:t2;
%theta = 0:179;
%theta = -180:179;
nu = (t2-t1)*1/st+1; 
%[R, xp] = radon(I,theta);

% Hough
% [H,theta,rho] = hough(BW,Name,Value) 计算有限角度范围内的 Hough 变换。
% 计算二值图像 BW 的标准 Hough 变换 (SHT)
% 该函数返回 rho（沿垂直于线条的向量从原点到线条的距离）和 theta（x 轴与该向量之间的角度，以度为单位）。
% 该函数还返回 SHT H，它是一个参数空间矩阵，其行和列分别对应于 rho 和 theta 值。
% Hough 变换矩阵，以大小为 nrho×ntheta 的数值矩阵形式返回。行和列对应于 rho 和 theta 值。
 [H, theta, xp] = hough(I, 'Theta', -90:89);
  R= -H;

 D = xp;
 T = theta;

if(Debug)
  figure;
  imagesc(theta,xp,R);
  title('Radon image, R_{\theta} (X\prime)');
  set(gcf,'name','Radon image','numbertitle','off');
  
  title('Hough image', 'FontSize',12); 
  xlabel('\theta (degrees)');
  ylabel('X\prime');
  set(gca,'XTick',-90:15:90);
  colormap(jet);
  colorbar
end

%[Y,ind] = max(max(R));
% Y 为矩阵中的最小值
[Y,ind] = min(min(R));
display(Y)
display(ind)
%ang = (t2-t1)*((ind-1)/(nu-1))+t1
ang = ind;
%figure;
%plot(max(R))

if(Debug)
  radonF = figure;
  axes1 = axes('Parent',radonF,'YDir','reverse');
  box(axes1,'on');
  hold(axes1,'all');
  plot(T, min(R))%'r--')

  title('extremum valuse for Radon transform for each angle')
  xlabel('\theta (degrees)');
  ylabel('Radon transform');
end